
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mp_roll_controller.c
  * @author     baiyang
  * @date       2023-6-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mp_roll_controller.h"

#include <ahrs/ahrs_view.h>
#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static float _get_rate_out(mp_roll_controller_t roll_ctrl, float desired_rate, float scaler, bool disable_integrator, bool ground_mode);
static void rctrl_assign_param(mp_roll_controller_t roll_ctrl);
/*----------------------------------variable----------------------------------*/
/* define parameters */
static param_t var_info[] = {
    // @Param: 2SRV_TCONST
    // @DisplayName: Roll Time Constant
    // @Description: Time constant in seconds from demanded to achieved roll angle. Most models respond well to 0.5. May be reduced for faster responses, but setting lower than a model can achieve will not help.
    // @Range: 0.4 1.0
    // @Units: s
    // @Increment: 0.1
    // @User: Advanced
    PARAM_DEFINE_FLOAT(RLL2SRV_TCONST,0.5f),

    // @Param: 2SRV_RMAX
    // @DisplayName: Maximum Roll Rate
    // @Description: This sets the maximum roll rate that the attitude controller will demand (degrees/sec) in angle stabilized modes. Setting it to zero disables this limit.
    // @Range: 0 180
    // @Units: deg/s
    // @Increment: 1
    // @User: Advanced
    PARAM_DEFINE_INT16(RLL2SRV_RMAX,0),

    PARAM_DEFINE_FLOAT(RLL_RATE_P,0.08f),
    PARAM_DEFINE_FLOAT(RLL_RATE_I,0.15f),
    PARAM_DEFINE_FLOAT(RLL_RATE_D,0),
    PARAM_DEFINE_FLOAT(RLL_RATE_FF,0.345f),
    PARAM_DEFINE_FLOAT(RLL_RATE_IMAX,0.666f),
    PARAM_DEFINE_FLOAT(RLL_RATE_FLTT,3.0f),
    PARAM_DEFINE_FLOAT(RLL_RATE_FLTE,0.0f),
    PARAM_DEFINE_FLOAT(RLL_RATE_FLTD,12.0f),
    PARAM_DEFINE_FLOAT(RLL_RATE_SMAX,150),
};

PARAM_GROUP_DEFINE(RLL_CTRL, var_info);
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void rctrl_ctor(mp_roll_controller_t roll_ctrl, mb_fixed_wing_const_t parms, float dt)
{
    roll_ctrl->aparm = parms;
    roll_ctrl->_dt = dt;

    pid_ctrl_ctor2(&roll_ctrl->rate_pid, 0.08f, 0.15f, 0, 0.345f, 0.666f, 3, 0, 12, 150, 1, dt);
    pid_ctrl_set_slew_limit_scale(&roll_ctrl->rate_pid, 45);

    rctrl_assign_param(roll_ctrl);
}

/*
 Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500
 A positive demand is up
 Inputs are:
 1) desired roll rate in degrees/sec
 2) control gain scaler = scaling_speed / aspeed
*/
float rctrl_get_rate_out(mp_roll_controller_t roll_ctrl, float desired_rate, float scaler)
{
    return _get_rate_out(roll_ctrl, desired_rate, scaler, false, false);
}

/*
 Function returns an equivalent aileron deflection in centi-degrees in the range from -4500 to 4500
 A positive demand is up
 Inputs are:
 1) demanded bank angle in centi-degrees
 2) control gain scaler = scaling_speed / aspeed
 3) boolean which is true when stabilise mode is active
 4) minimum FBW airspeed (metres/sec)
*/
float rctrl_get_servo_out(mp_roll_controller_t roll_ctrl, int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode)
{
    if (roll_ctrl->gains.tau < 0.05f) {
        roll_ctrl->gains.tau = 0.05f;
    }

    // Calculate the desired roll rate (deg/sec) from the angle error
    roll_ctrl->angle_err_deg = angle_err * 0.01f;
    float desired_rate = roll_ctrl->angle_err_deg/ roll_ctrl->gains.tau;

    // Limit the demanded roll rate
    if (roll_ctrl->gains.rmax_pos && desired_rate < -roll_ctrl->gains.rmax_pos) {
        desired_rate = - roll_ctrl->gains.rmax_pos;
    } else if (roll_ctrl->gains.rmax_pos && desired_rate > roll_ctrl->gains.rmax_pos) {
        desired_rate = roll_ctrl->gains.rmax_pos;
    }

    return _get_rate_out(roll_ctrl, desired_rate, scaler, disable_integrator, ground_mode);
}

/*
  start an autotune
 */
void rctrl_autotune_start(mp_roll_controller_t roll_ctrl)
{
    if (roll_ctrl->autotune == NULL) {
        roll_ctrl->autotune = mp_autotune_probe(&roll_ctrl->gains, MP_AUTOTUNE_ROLL, roll_ctrl->aparm, &roll_ctrl->rate_pid, roll_ctrl->_dt);
        if (roll_ctrl->autotune == NULL) {
            if (!roll_ctrl->failed_autotune_alloc) {
                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed roll allocation");
            }
            roll_ctrl->failed_autotune_alloc = true;
        }
    }
    if (roll_ctrl->autotune != NULL) {
        mp_autotune_start(roll_ctrl->autotune);
    }
}

/*
  restore autotune gains
 */
void rctrl_autotune_restore(mp_roll_controller_t roll_ctrl)
{
    if (roll_ctrl->autotune != NULL) {
        mp_autotune_stop(roll_ctrl->autotune);
    }
}

/*
  AC_PID based rate controller
*/
static float _get_rate_out(mp_roll_controller_t roll_ctrl, float desired_rate, float scaler, bool disable_integrator, bool ground_mode)
{
    ahrs_view *_ahrs = get_ahrs_view();

    const float eas2tas = ahrs_get_EAS2TAS(_ahrs);
    bool limit_I = fabsf(roll_ctrl->_last_out) >= 45;
    float rate_x = _ahrs->gyr.x;
    float aspeed;
    float old_I = pid_ctrl_get_i(&roll_ctrl->rate_pid);

    if (!ahrs_airspeed_estimate(_ahrs, &aspeed)) {
        aspeed = 0;
    }
    bool underspeed = aspeed <= (float)(roll_ctrl->aparm->airspeed_min);
    if (underspeed) {
        limit_I = true;
    }

    // the P and I elements are scaled by sq(scaler). To use an
    // unmodified AC_PID object we scale the inputs and calculate FF separately
    //
    // note that we run AC_PID in radians so that the normal scaling
    // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
    pid_ctrl_update_all(&roll_ctrl->rate_pid, radians(desired_rate) * scaler * scaler, rate_x * scaler * scaler, limit_I);

    if (underspeed) {
        // when underspeed we lock the integrator
        pid_ctrl_set_integrator1(&roll_ctrl->rate_pid, old_I);
    }

    // FF should be scaled by scaler/eas2tas, but since we have scaled
    // the AC_PID target above by scaler*scaler we need to instead
    // divide by scaler*eas2tas to get the right scaling
    const float ff = degrees(pid_ctrl_get_ff(&roll_ctrl->rate_pid) / (scaler * eas2tas));

    if (disable_integrator) {
        pid_ctrl_reset_i(&roll_ctrl->rate_pid);
    }

    // convert AC_PID info object to same scale as old controller
    roll_ctrl->_pid_info = roll_ctrl->rate_pid._pid_info;

    const float deg_scale = degrees(1);
    roll_ctrl->_pid_info.FF = ff;
    roll_ctrl->_pid_info.P *= deg_scale;
    roll_ctrl->_pid_info.I *= deg_scale;
    roll_ctrl->_pid_info.D *= deg_scale;

    // fix the logged target and actual values to not have the scalers applied
    roll_ctrl->_pid_info.target = desired_rate;
    roll_ctrl->_pid_info.actual = degrees(rate_x);

    // sum components
    float out = roll_ctrl->_pid_info.FF + roll_ctrl->_pid_info.P + roll_ctrl->_pid_info.I + roll_ctrl->_pid_info.D;
    if (ground_mode) {
        // when on ground suppress D term to prevent oscillations
        out -= roll_ctrl->_pid_info.D + 0.5f*roll_ctrl->_pid_info.P;
    }

    // remember the last output to trigger the I limit
    roll_ctrl->_last_out = out;

    if (roll_ctrl->autotune != NULL && roll_ctrl->autotune->running && aspeed > roll_ctrl->aparm->airspeed_min) {
        // let autotune have a go at the values
        mp_autotune_update(roll_ctrl->autotune, &roll_ctrl->_pid_info, scaler, roll_ctrl->angle_err_deg);
    }

    // output is scaled to notional centidegrees of deflection
    return math_constrain_float(out * 100, -4500, 4500);
}

/**
  * @brief       
  * @param[in]   roll_ctrl  
  * @param[out]  
  * @retval      
  * @note        
  */
static void rctrl_assign_param(mp_roll_controller_t roll_ctrl)
{
    param_link_variable(PARAM_ID(RLL_CTRL, RLL2SRV_TCONST), &roll_ctrl->gains.tau);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL2SRV_RMAX), &roll_ctrl->gains.rmax_pos);

    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_P), &roll_ctrl->rate_pid._kp);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_I), &roll_ctrl->rate_pid._ki);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_D), &roll_ctrl->rate_pid._kd);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_FF), &roll_ctrl->rate_pid._kff);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_IMAX), &roll_ctrl->rate_pid._kimax);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_FLTT), &roll_ctrl->rate_pid._filt_t_hz);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_FLTE), &roll_ctrl->rate_pid._filt_e_hz);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_FLTD), &roll_ctrl->rate_pid._filt_d_hz);
    param_link_variable(PARAM_ID(RLL_CTRL, RLL_RATE_SMAX), &roll_ctrl->rate_pid._slew_rate_max);
}

/*------------------------------------test------------------------------------*/


